clc;
clear all;
close all;

J = 2;  
kp = 1.2;  ki = 3.1;  kd = 0.08;    lamda = 1.2;    beta = 1.0;
alpha = 0.001;  fai = 0.01;

x0 = [0;0;0;0;0];


[t,x] = ode45('MYplant',[0 20],x0,[]);

theta   = x(:,1);
dtheta  = x(:,2);
s       = x(:,3);
ds      = x(:,4);
gama_h  = x(:,5);

theta_d     = sin(t);
dtheta_d    = cos(t);
ddx1d       = -sin(t);
e = theta_d - theta;
de= dtheta_d- dtheta;
ut = zeros(length(t),1);
% for i=1:length(t)
%     ut(i) = J/kd * ( -s(i)-beta*ds(i)+kp*de(i)+ki*e(i)+kd*ddx1d(i)+lamda(i)*s(i)+gama_h(i)*sign(ds(i)) );
% end
ut = J/kd * ( -beta.*ds+kp.*de+ki.*e+kd.*ddx1d+lamda.*s+gama_h.*saturate(ds/fai,-1,1) );

figure(1);
subplot(211);
plot(t,theta_d,'r',t,theta,'k:','linewidth',2);
xlabel('time(s)');ylabel('theta');
legend('ideal  signal','tracking signal');
subplot(212);
plot(t,e,'r','linewidth',2);
xlabel('time(s)');ylabel('position tracking error');
legend('tracking error');


figure(2);
subplot(211);
plot(t,dtheta_d,'r',t,dtheta,'k:','linewidth',2);
xlabel('time(s)');ylabel('dtheta');
legend('ideal  signal','tracking signal');
subplot(212);
plot(t,de,'r','linewidth',2);
xlabel('time(s)');ylabel('velocity tracking error');
legend('tracking error');

figure(3);
plot(t,s,'r','linewidth',2);
xlabel('time(s)');ylabel('s');
legend('sliding veriable');

figure(4);
plot(t,gama_h,'r','linewidth',2);
xlabel('time(s)');ylabel('gama_h');
legend('adaptive gain');

figure(5);
plot(t,ut,'r','linewidth',2);
xlabel('time(s)');ylabel('ut');
legend('ut');